﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;


namespace DDD.Core {
    using Vector4 = Vector4<float>;

    /// <summary>
    /// 4x4行列を表す構造体
    /// </summary>
    /// <remarks>
    /// 一般的な4x4行列です。<see cref="Vector4"/>型のベクトルを<dic>V'=MV</dic>で変換可能です。 
    /// アニメート可能、エクスポート可能。
    /// </remarks>
    public struct Matrix4x4 : IExportable<float>, IAnimatable<float>, IEquatable<Matrix4x4> {


        #region Constructor
        /// <summary>
        /// コンストラクター
        /// </summary>
        /// <remarks>
        /// 標準的な4x4行列です。3次元のアフィン変換を表します。
        /// 行優先で指定します。
        /// </remarks>
        /// <param name="m00">M00要素</param>
        /// <param name="m01">M01要素</param>
        /// <param name="m02">M02要素</param>
        /// <param name="m03">M03要素</param>
        /// <param name="m10">M10要素</param>
        /// <param name="m11">M11要素</param>
        /// <param name="m12">M12要素</param>
        /// <param name="m13">M13要素</param>
        /// <param name="m20">M20要素</param>
        /// <param name="m21">M21要素</param>
        /// <param name="m22">M22要素</param>
        /// <param name="m23">M23要素</param>
        /// <param name="m30">M30要素</param>
        /// <param name="m31">M31要素</param>
        /// <param name="m32">M32要素</param>
        /// <param name="m33">M33要素</param>
        public Matrix4x4 (float m00, float m01, float m02, float m03,
                          float m10, float m11, float m12, float m13,
                          float m20, float m21, float m22, float m23,
                          float m30, float m31, float m32, float m33)
            : this () {
            this.M00 = m00;
            this.M01 = m01;
            this.M02 = m02;
            this.M03 = m03;
            this.M10 = m10;
            this.M11 = m11;
            this.M12 = m12;
            this.M13 = m13;
            this.M20 = m20;
            this.M21 = m21;
            this.M22 = m22;
            this.M23 = m23;
            this.M30 = m30;
            this.M31 = m31;
            this.M32 = m32;
            this.M33 = m33;
        }

        /// <summary>
        /// 要素を配列で指定するコンストラクター
        /// </summary>
        /// 配列で指定して4x4行列を作成します。
        /// 配列はすぐさま内部にコピーされます。
        /// <param name="m">16要素の配列</param>
        public Matrix4x4 (float[] m)
            : this () {
            if (m == null) {
                throw new ArgumentNullException ("Matrix is null");
            }
            if (m.Length < 16) {
                throw new ArgumentException ("Matrix is too short");
            }
            this.M00 = m[0];
            this.M01 = m[1];
            this.M02 = m[2];
            this.M03 = m[3];
            this.M10 = m[4];
            this.M11 = m[5];
            this.M12 = m[6];
            this.M13 = m[7];
            this.M20 = m[8];
            this.M21 = m[9];
            this.M22 = m[10];
            this.M23 = m[11];
            this.M30 = m[12];
            this.M31 = m[13];
            this.M32 = m[14];
            this.M33 = m[15];
        }
        #endregion

        #region Property

        /// <summary>
        /// M00要素
        /// </summary>
        public float M00 {
            get;
            private set;
        }

        /// <summary>
        /// M01要素
        /// </summary>
        public float M01 {
            get;
            private set;
        }

        /// <summary>
        /// M02要素
        /// </summary>
        public float M02 {
            get;
            private set;
        }

        /// <summary>
        /// M03要素
        /// </summary>
        public float M03 {
            get;
            private set;
        }

        /// <summary>
        /// M10要素
        /// </summary>
        public float M10 {
            get;
            private set;
        }

        /// <summary>
        /// M11要素
        /// </summary>
        public float M11 {
            get;
            private set;
        }

        /// <summary>
        /// M12要素
        /// </summary>
        public float M12 {
            get;
            private set;
        }

        /// <summary>
        /// M13要素
        /// </summary>
        public float M13 {
            get;
            private set;
        }

        /// <summary>
        /// M20要素
        /// </summary>
        public float M20 {
            get;
            private set;
        }

        /// <summary>
        /// M21要素
        /// </summary>
        public float M21 {
            get;
            private set;
        }

        /// <summary>
        /// M22要素
        /// </summary>

        public float M22 {
            get;
            private set;
        }

        /// <summary>
        /// M23要素
        /// </summary>

        public float M23 {
            get;
            private set;
        }

        /// <summary>
        /// M30要素
        /// </summary>

        public float M30 {
            get;
            private set;
        }

        /// <summary>
        /// M31要素
        /// </summary>
        public float M31 {
            get;
            private set;
        }

        /// <summary>
        /// M32要素
        /// </summary>

        public float M32 {
            get;
            private set;
        }

        /// <summary>
        /// M33要素
        /// </summary>

        public float M33 {
            get;
            private set;
        }

        /// <summary>
        /// コンポーネント数
        /// </summary>
        public int ComponentCount {
            get { return 16; }
        }

        /// <summary>
        /// コンポーネントにアクセスするインデクサー
        /// </summary>
        /// <param name="index">インデックス</param>
        /// <returns></returns>
        public float this[int index] {
            get {
                if (index < 0 || index >= ComponentCount) {
                    throw new IndexOutOfRangeException ("Index is out of range");
                }
                switch (index) {
                    case 0: return M00;
                    case 1: return M01;
                    case 2: return M02;
                    case 3: return M03;
                    case 4: return M10;
                    case 5: return M11;
                    case 6: return M12;
                    case 7: return M13;
                    case 8: return M20;
                    case 9: return M21;
                    case 10: return M22;
                    case 11: return M23;
                    case 12: return M30;
                    case 13: return M31;
                    case 14: return M32;
                    case 15: return M33;
                    default: return 0;
                }
            }
            set {
                if (index < 0 || index >= ComponentCount) {
                    throw new IndexOutOfRangeException ("Index is out of range");
                }
                switch (index) {
                    case 0: M00 = value; break;
                    case 1: M01 = value; break;
                    case 2: M02 = value; break;
                    case 3: M03 = value; break;
                    case 4: M10 = value; break;
                    case 5: M11 = value; break;
                    case 6: M12 = value; break;
                    case 7: M13 = value; break;
                    case 8: M20 = value; break;
                    case 9: M21 = value; break;
                    case 10: M22 = value; break;
                    case 11: M23 = value; break;
                    case 12: M30 = value; break;
                    case 13: M31 = value; break;
                    case 14: M32 = value; break;
                    case 15: M33 = value; break;
                    default: break;
                }
            }
        }

        /// <summary>
        /// この行列の行列式
        /// </summary>
        public float Deternimant {
            get {
                return M00 * M11 * M22 * M33 + M00 * M12 * M23 * M31 + M00 * M13 * M21 * M32
                       + M01 * M10 * M23 * M32 + M01 * M12 * M20 * M33 + M01 * M13 * M22 * M30
                       + M02 * M10 * M21 * M33 + M02 * M11 * M23 * M30 + M02 * M13 * M20 * M31
                       + M03 * M10 * M22 * M31 + M03 * M11 * M20 * M32 + M03 * M12 * M21 * M30
                       - M00 * M11 * M23 * M32 - M00 * M12 * M21 * M33 - M00 * M13 * M22 * M31
                       - M01 * M10 * M22 * M33 - M01 * M12 * M23 * M30 - M01 * M13 * M20 * M32
                       - M02 * M10 * M23 * M31 - M02 * M11 * M20 * M33 - M02 * M13 * M21 * M30
                       - M03 * M10 * M21 * M32 - M03 * M11 * M22 * M30 - M03 * M12 * M20 * M31;
            }
        }

        /// <summary>
        /// この行列の3x3要素
        /// </summary>
        /// <remarks>
        /// この行列の左上3x3要素を取り出します。
        /// 残りの成分は無視されます。
        /// </remarks>
        public Matrix3x3 Matrix3x3 {
            get {
                return new Matrix3x3 (M00, M01, M02,
                                      M10, M11, M12,
                                      M20, M21, M22);
            }
        }

        /// <summary>
        /// 全要素をfloatの配列にキャストします
        /// </summary>
        /// <param name="m">変換元の4x4行列</param>
        /// <returns>変換後のfloatの配列</returns>
        public static explicit operator float[] (Matrix4x4 m) {
            return new float[] { m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8], m[9], m[10], m[11], m[12], m[13], m[14], m[15] };
        }

        /// <summary>
        /// 単位行列
        /// </summary>
        /// <returns>新しく作成した単位行列</returns>
        public static Matrix4x4 Identity {
            get {
                return new Matrix4x4 (1, 0, 0, 0,
                                  0, 1, 0, 0,
                                  0, 0, 1, 0,
                                  0, 0, 0, 1);
            }
        }
        #endregion

        #region Method


        /// <summary>
        ///  この行列の逆行列を作成します
        /// </summary>
        /// <returns>新しく作成した逆行列</returns>
        public Matrix4x4 Inverse () {
            float det = Deternimant;
            if (det == 0) {
                throw new InvalidOperationException ("Can't invert this matrix");
            }
            var m = new Matrix4x4 ();
            m.M00 = 1 / det * (M11 * M22 * M33 + M12 * M23 * M31 + M13 * M21 * M32 - M11 * M23 * M32 - M12 * M21 * M33 - M13 * M22 * M31);
            m.M01 = 1 / det * (M01 * M23 * M32 + M02 * M21 * M33 + M03 * M22 * M31 - M01 * M22 * M33 - M02 * M23 * M31 - M03 * M21 * M32);
            m.M02 = 1 / det * (M01 * M12 * M33 + M02 * M13 * M31 + M03 * M11 * M32 - M01 * M13 * M32 - M02 * M11 * M33 - M03 * M12 * M31);
            m.M03 = 1 / det * (M01 * M13 * M22 + M02 * M11 * M23 + M03 * M12 * M21 - M01 * M12 * M23 - M02 * M13 * M21 - M03 * M11 * M22);
            m.M10 = 1 / det * (M10 * M23 * M32 + M12 * M20 * M33 + M13 * M22 * M30 - M10 * M22 * M33 - M12 * M23 * M30 - M13 * M20 * M32);
            m.M11 = 1 / det * (M00 * M22 * M33 + M02 * M23 * M30 + M03 * M20 * M32 - M00 * M23 * M32 - M02 * M20 * M33 - M03 * M22 * M30);
            m.M12 = 1 / det * (M00 * M13 * M32 + M02 * M10 * M33 + M03 * M12 * M30 - M00 * M12 * M33 - M02 * M13 * M30 - M03 * M10 * M32);
            m.M13 = 1 / det * (M00 * M12 * M23 + M02 * M13 * M20 + M03 * M10 * M22 - M00 * M13 * M22 - M02 * M10 * M23 - M03 * M12 * M20);
            m.M20 = 1 / det * (M10 * M21 * M33 + M11 * M23 * M30 + M13 * M20 * M31 - M10 * M23 * M31 - M11 * M20 * M33 - M13 * M21 * M30);
            m.M21 = 1 / det * (M00 * M23 * M31 + M01 * M20 * M33 + M03 * M21 * M30 - M00 * M21 * M33 - M01 * M23 * M30 - M03 * M20 * M30);
            m.M22 = 1 / det * (M00 * M11 * M33 + M01 * M13 * M30 + M03 * M10 * M31 - M00 * M13 * M31 - M01 * M10 * M33 - M03 * M11 * M30);
            m.M23 = 1 / det * (M00 * M13 * M21 + M01 * M10 * M23 + M03 * M11 * M20 - M00 * M11 * M23 - M01 * M13 * M20 - M03 * M10 * M21);
            m.M30 = 1 / det * (M10 * M22 * M31 + M11 * M20 * M32 + M12 * M21 * M30 - M10 * M21 * M32 - M11 * M22 * M30 - M12 * M20 * M31);
            m.M31 = 1 / det * (M00 * M21 * M32 + M01 * M22 * M30 + M02 * M20 * M31 - M00 * M22 * M31 - M01 * M20 * M32 - M02 * M21 * M30);
            m.M32 = 1 / det * (M00 * M12 * M31 + M01 * M10 * M32 + M02 * M11 * M30 - M00 * M11 * M32 - M01 * M12 * M30 - M02 * M10 * M31);
            m.M33 = 1 / det * (M00 * M11 * M22 + M01 * M12 * M20 + M02 * M10 * M21 - M00 * M12 * M21 - M01 * M10 * M22 - M02 * M11 * M20);

            return m;
        }

        /// <summary>
        /// この行列を転置した新しい行列を作成します
        /// </summary>
        /// <returns>新しく作成した転置行列</returns>
        public Matrix4x4 Transpose () {
            return new Matrix4x4 (M00, M10, M20, M30,
                                  M01, M11, M21, M31,
                                  M02, M12, M22, M32,
                                  M03, M13, M23, M33);
        }



        /// <summary>
        /// 平行移動を表す4x4行列を作成します
        /// </summary>
        /// <param name="tx">X方向の平行移動量</param>
        /// <param name="ty">Y方向の平行移動量</param>
        /// <param name="tz">Z方向の平行移動量</param>
        /// <returns></returns>
        public static Matrix4x4 Translate (float tx, float ty, float tz) {
            return new Matrix4x4 (1, 0, 0, tx,
                                  0, 1, 0, ty,
                                  0, 0, 1, tz,
                                  0, 0, 0, 1);
        }

        /// <summary>
        /// スケーリングを表す4x4行列を作成します
        /// </summary>
        /// <param name="sx">X方向の拡大量</param>
        /// <param name="sy">Y方向の拡大量</param>
        /// <param name="sz">Z方向の拡大量</param>
        /// <returns></returns>
        public static Matrix4x4 Scale (float sx, float sy, float sz) {
            return new Matrix4x4 (sx, 0, 0, 0,
                                  0, sy, 0, 0,
                                  0, 0, sz, 0,
                                  0, 0, 0, 1);
        }

        /// <summary>
        /// 回転を表す4x4行列を作成します
        /// </summary>
        /// <remarks>
        /// 回転はAngle-Axis形式で指定します。
        /// 回転角度は度数(in deree[0,360))で指定し、
        /// 回転軸は3次元のベクトル(正規化されている必要はない)で指定します。
        /// <code>Angle=0, Axis=(0,0,0)</code>は有効です。
        /// </remarks>
        /// <param name="angle">回転角度(in deree[0,360))</param>
        /// <param name="ax">回転軸のX成分</param>
        /// <param name="ay">回転軸のY成分</param>
        /// <param name="az">回転軸のZ成分</param>
        /// <returns></returns>
        public static Matrix4x4 Rotate (float angle, float ax, float ay, float az) {
            angle = (float)Math.PI * angle / 180.0f;

            float c = (float)Math.Cos (angle);
            float s = (float)Math.Sin (angle);

            float m00 = ax * ax * (1 - c) + c;
            float m01 = ax * ay * (1 - c) - az * s;
            float m02 = ax * az * (1 - c) + ay * s;
            float m10 = ay * ax * (1 - c) + az * s;
            float m11 = ay * ay * (1 - c) + c;
            float m12 = ay * az * (1 - c) - ax * s;
            float m20 = ax * az * (1 - c) - ay * s;
            float m21 = ay * az * (1 - c) + ax * s;
            float m22 = az * az * (1 - c) + c;

            return new Matrix4x4 (m00, m01, m02, 0,
                                  m10, m11, m12, 0,
                                  m20, m21, m22, 0,
                                  0, 0, 0, 1);
        }

        /// <summary>
        /// 指定のクォータニオンと等価な4x4行列を新規に作成します
        /// </summary>
        /// <param name="q">クォータニオン</param>
        /// <returns>4x4行列</returns>
        public static Matrix4x4 Rotate (Quaternion q) {
            var m = q.Matrix3x3;
            return new Matrix4x4(m.M00, m.M01, m.M02, 0,
                                 m.M10, m.M11, m.M12, 0,
                                 m.M20, m.M21, m.M22, 0,
                                 0, 0, 0, 1);
        }



        /// <summary>
        /// この4x4行列を使って4要素のベクトルを変換します。<code>V'=MV</code>.
        /// </summary>
        /// <param name="m">変換行列</param>
        /// <param name="v">ベクトル</param>
        /// <returns>変換後のベクトル</returns>
        public static Vector4 operator * (Matrix4x4 m, Vector4 v) {
            float v0 = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3] * v[3];
            float v1 = m[4] * v[0] + m[5] * v[1] + m[6] * v[2] + m[7] * v[3];
            float v2 = m[8] * v[0] + m[9] * v[1] + m[10] * v[2] + m[11] * v[3];
            float v3 = m[12] * v[0] + m[13] * v[1] + m[14] * v[2] + m[15] * v[3];
            return new Vector4 (v0, v1, v2, v3);
        }

        /// <summary>
        /// 行列同士のかけ算<code>M'=M1*M2</code>.
        /// </summary>
        /// <param name="m1">行列1</param>
        /// <param name="m2">行列2</param>
        /// <returns>行列責</returns>
        public static Matrix4x4 operator * (Matrix4x4 m1, Matrix4x4 m2) {
            float m00 = m1[0] * m2[0] + m1[1] * m2[4] + m1[2] * m2[8] + m1[3] * m2[12];
            float m01 = m1[0] * m2[1] + m1[1] * m2[5] + m1[2] * m2[9] + m1[3] * m2[13];
            float m02 = m1[0] * m2[2] + m1[1] * m2[6] + m1[2] * m2[10] + m1[3] * m2[14];
            float m03 = m1[0] * m2[3] + m1[1] * m2[7] + m1[2] * m2[11] + m1[3] * m2[15];
            float m10 = m1[4] * m2[0] + m1[5] * m2[4] + m1[6] * m2[8] + m1[7] * m2[12];
            float m11 = m1[4] * m2[1] + m1[5] * m2[5] + m1[6] * m2[9] + m1[7] * m2[13];
            float m12 = m1[4] * m2[2] + m1[5] * m2[6] + m1[6] * m2[10] + m1[7] * m2[14];
            float m13 = m1[4] * m2[3] + m1[5] * m2[7] + m1[6] * m2[11] + m1[7] * m2[15];
            float m20 = m1[8] * m2[0] + m1[9] * m2[4] + m1[10] * m2[8] + m1[11] * m2[12];
            float m21 = m1[8] * m2[1] + m1[9] * m2[5] + m1[10] * m2[9] + m1[11] * m2[13];
            float m22 = m1[8] * m2[2] + m1[9] * m2[6] + m1[10] * m2[10] + m1[11] * m2[14];
            float m23 = m1[8] * m2[3] + m1[9] * m2[7] + m1[10] * m2[11] + m1[11] * m2[15];
            float m30 = m1[12] * m2[0] + m1[13] * m2[4] + m1[14] * m2[8] + m1[15] * m2[12];
            float m31 = m1[12] * m2[1] + m1[13] * m2[5] + m1[14] * m2[9] + m1[15] * m2[13];
            float m32 = m1[12] * m2[2] + m1[13] * m2[6] + m1[14] * m2[10] + m1[15] * m2[14];
            float m33 = m1[12] * m2[3] + m1[13] * m2[7] + m1[14] * m2[11] + m1[15] * m2[15];
            return new Matrix4x4 (m00, m01, m02, m03,
                                  m10, m11, m12, m13,
                                  m20, m21, m22, m23,
                                  m30, m31, m32, m33);
        }


        /// <summary>
        /// 同値性を比較します
        /// </summary>
        /// <remarks>
        /// 同値性を比較する汎用の比較関数です。
        /// <see cref="Matrix4x4"/>オブジェクトはコンポーネント要素が厳密に一致しなくても、
        /// 全てのコンポーネント要素で差が<see cref="GlobalSettings.Tolerance"/>以下であれば等しいと見なされます。
        /// </remarks>
        /// <param name="obj">右辺のオブジェクト</param>
        /// <returns>等しいときtrue, そうでないときfalse</returns>
        public override bool Equals (object obj) {
            if (object.ReferenceEquals (obj, null)) {
                return false;
            }
            if (object.ReferenceEquals (obj, this)) {
                return true;
            }
            var m = obj as Matrix4x4?;
            if (m == null) {
                return false;
            }

            return this.Equals (m.Value);
        }

        /// <summary>
        /// タイプセーフな同値性を比較します
        /// </summary>
        /// <remarks>
        /// 
        /// </remarks>
        /// <param name="m">右辺の<see cref="Matrix4x4"/>オブジェクト</param>
        /// <returns>等しいときtrue, そうでないときfalse</returns>
        public bool Equals (Matrix4x4 m) {
            for (int i = 0; i < ComponentCount; i++) {
                if (Math.Abs (this[i] - m[i]) >= GlobalSettings.Tolerance) {
                    return false;
                }
            }
            return true;
        }

        /// <summary>
        /// 等号による同値性を比較します
        /// </summary>
        /// <param name="left">左項</param>
        /// <param name="right">右項</param>
        /// <returns>等しいときtrue, そうでないときfalse</returns>
        public static bool operator == (Matrix4x4 left, Matrix4x4 right) {
            return left.Equals (right);
        }

        /// <summary>
        /// 等号による同値性を比較します
        /// </summary>
        /// <param name="left">左項</param>
        /// <param name="right">右項</param>
        /// <returns>等しくないときtrue, そうでないときfalse</returns>
        public static bool operator != (Matrix4x4 left, Matrix4x4 right) {
            return !(left == right);
        }

        /// <summary>
        /// 一意のハッシュ値を取得します
        /// </summary>
        /// <remarks>
        /// このメソッドは同値性の比較メソッドと異なり
        /// <see cref="GlobalSettings.Tolerance"/>の影響を受けません。
        /// </remarks>
        /// <returns></returns>
        public override int GetHashCode () {
            return base.GetHashCode ();
        }

        /// <summary>
        /// 現在のオブジェクトの状態を表す文字列を返します。
        /// </summary> 
        public override string ToString () {
            return string.Format ("Matrix4x4 : {0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, {9}, {10}, {11}, {12}, {13}, {14}, {15}", M00, M01, M02, M03, M10, M11, M12, M13, M20, M21, M22, M23, M30, M31, M32, M33);
        }


        #endregion

    }
}
